/*
* (c) 2014 University of Applied Sciences, Karlsruhe
* Project "Segmentation of depth data of a plenoptic camera"
* summer semester 2014
*
* fit_line_3d.h
* This file contains an abstract class which provides the basic interface to fit 2D contours into 3D world data. Besides that, it provides additional helper functions
* the algorithms used by all possible derivatives of this abstract class.
*/

#ifndef _FIT_LINE_3D_H_
#define _FIT_LINE_3D_H_

#include <ostream>
#include <vector>

#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <pcl/common/distances.h>

#include "sgBase/misc/histogram.h"
#include "sgBase/visualization_3d.h"
#include "sgBase/geometrics/line_3d.h"


using namespace sgBase;

namespace sgMain
{
	/**
	Selection of the depth fitting algorithm. See the project documentation for more details how these fitting algorithms work.
	*/
	enum Segmentation3DDepthFittingMethod
	{
		FITTING_MINIMUM_VARIANCE,
		FITTING_LEAST_SQUARES,
		FITTING_BEGIN_AND_END_POINT_AVERAGING,
		FITTING_NONE
	};

	/**
	Abstract class to perform the contour depth fitting. For each 2D line, an instance of this class must be created.
	*/
	class FitLine3D
	{
	protected:
		Segmentation3DDepthFittingMethod fittingMethod; /// The fitting method used by this class.
		bool isFitted; /// Flag to identify wether this linehas already been fitted and if yes, if the fitting was succssful
		
		cv::Mat intrinsics; /// The camera's intrinsic parameters.
		Line3D line2D; /// The 2D line which is to be fitted.

		pcl::PointCloud<pcl::PointXYZRGB>::Ptr depthCloud; /// The depth data in camera space.
		double radius; /// The depth point search radius around the 2D contour line.

		/**
		overloadable function to print the fitted line data into an output stream.
		*/
		virtual void print(std::ostream& os) const;
	public:
		/**
		Constructor.
		\param[in] line2D The 2D contour line.
		\param[in] depthCloud The depth data in point-cloud representation (in world coordinates).
		\param[in] intrinsicParameters The camera's intrinsic parameters.
		\param[in] radius The maximum distance between the contour 2D line and the depth data points which are selected for the fitting process.
		*/
		FitLine3D(const Line3D &line2D, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &depthCloud, const cv::Mat &intrinsicParameters, double radius);

		/**
		Pure virtual destructor.
		*/
		virtual ~FitLine3D() = 0;


		pcl::PointCloud<pcl::PointXYZRGB>::Ptr lineCloud; /// the selected points of the depth data around the 2D line.

		Line3D fittedLine; /// The result of the fitting process - the line in 3D coordinates.
		
		/**
		Function to perform the line fitting process.
		\return 0 if successful, otherwise non-zero.
		*/
		virtual int FitLine() = 0;
		
		/**
		Output stream operator.
		*/
		friend std::ostream& operator<< (std::ostream& os, const FitLine3D& line);

		/**
		Returns the variance of the dinstances of the selected depth points and the fitted line.
		\return 0.0 if no fitting has been done, otherwise the variance.
		*/
		virtual double GetVariance() const;

		/**
		Returns a histogram of the distances between the fitted line and the selected depth points.
		\param[in] numberOfBins	The number of bins used in this histogram.
		*/
		virtual Histogram GetHistogram(unsigned int numberOfBins) const;

		/**
		Returns whether a 3D contour fit has been found or not.
		*/
		virtual bool IsFitted() const;

		/**
		Renders the fitted line into a 3D viewport. Each line required an unique index, so make sure to not give the 
		same contour and line index twice.
		\param[in] viewport3D The viewport in which this line should be rendered.
		\param[in] contourIndex The index of the contour to which this line belongs to.
		\param[in] lineIndex The index of this line in the 3D contour it belongs to.
		*/
		virtual void Render(Visualization3D &viewport3D, int contourIndex, int lineIndex) const;
	};

	/**
	Output steam operator for the fitted line object.
	*/
	std::ostream& operator<< (std::ostream& os, const FitLine3D& line);

	namespace FitLine3DHelper
	{
		/**
		Fits a line into a 3D world point cloud by taking the average depth values at two points close to the line begin and end point.
		\param[in] line2D The 2D line.
		\param[in] depthCloud The point cloud to fit the line into (in camera coordinates).

		\param[in] intrinsicParameters The matrix of intrinsic parameters of the camera, necessary to project the 2D line into 3D space.
		\param[in] maxDistancePointToLine The maximum distance to use a depth point for the fitting process.
		\param[out] result The calculated 3D line.
		\return 0 if successful, -1 if not enough points could be found within the search radius.
		*/
		int FitLineIntoPointCloudByAveragingBeginAndEndPoints(const Line3D &line2D, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &depthCloud, const cv::Mat &intrinsicParameters, double maxDistancePointToLine, Line3D &result);
		
		/**
		Fits a line given in 2D coordinates into a 3D point cloud using least-squares fitting.
		\param[in] line2D The 2D line.
		\param[out] output The calculated 3D line.
		\param[in] intrinsic The matrix of intrinsic parameters of the camera, necessary to project the 2D line into 3D space
		\param[in] depthCloud The point cloud to fit the line into (in camera coordinates).
		\return 0 if successful, otherwise nonzero
		*/
		int FitLine2DIntoPointCloud3DLeastSquares(const Line3D &line2D, Line3D &output, const cv::Mat &intrinsic, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointCloud);
		
		/**
		Fits a line given in 2D coordinates into a 3D point cloud using a minimum variance fitting algorithm to add addtional weights for the fitting algorithm.
		For more details, please refer to the project documentation.
		\param[in] line2D The 2D line.
		\param[out] output The calculated 3D line.
		\param[in] intrinsic The matrix of intrinsic parameters of the camera, necessary to project the 2D line into 3D space
		\param[in] points List of points which are used to perform the fitting process.
		\param[in] pointImportance List of values which represent the weight of the points. The higher the value, the more this point is weighted.
		\return 0 if successful, -1 if the MV algorithm was not able to find a solution, -2 if the fitting result did not pass the logic validation test (= results do not make sense)
		*/
		int FitLine2DIntoPointCloud3DMinimumVariance(const Line3D &line2D, Line3D &output, const cv::Mat &intrinsic, const std::vector<Eigen::Vector3f> &points, const std::vector<unsigned int> &pointImportance);

		/**
		Fits a 2D line of infinite length into a set of 2D points using an least squares algorithm.
		\param[in] points The list of 2D points which are used for the fitting process. z coordinates are ignored.
		\param[out] linePoint A point on the fitted line.
		\param[out] lineDirection The direction vector of the fitted line.
		\return 0 if successful, -1 if the system is not solveable (determinant = 0), -2 if the result NaN
		*/
		int FitLine2DIntoPoints2DLeastSquares(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &points, Eigen::Vector2f &linePoint, Eigen::Vector2f &lineDirection);

		/**
		Fits a 2D line of infinite length into a set of 2D points using an least squares algorithm.
		\param[in] points The list of 2D points which are used for the fitting process. z coordinates are ignored.
		\param[out] linePoint A point on the fitted line.
		\param[out] lineDirection The direction vector of the fitted line.
		\return 0 if successful, -1 if the system is not solveable (determinant = 0), -2 if the result NaN
		*/
		int FitLine2DIntoPoints2DLeastSquares(const std::vector<Eigen::Vector3f> &points, Eigen::Vector2f &linePoint, Eigen::Vector2f &lineDirection);

		/**
		Fits a 2D line of infinite length into a set of 2D points using an minimum variance algorithm.
		\param[in] points The list of 2D points which are used for the fitting process. z coordinates are ignored.
		\param[in] pointImportance The weight of the points. The higher the value the stronger is its impact on the solution.
		\param[out] linePoint A point on the fitted line.
		\param[out] lineDirection The direction vector of the fitted line.
		\return 0 if successful, -1 if points and pointImportance do not have the same size, -2 if the system is not solveable (determinant = 0), -3 if the result NaN
		*/
		int FitLine2DIntoPoints2DMinimumVariance(const std::vector<Eigen::Vector3f> &points, const std::vector<unsigned int> &pointImportance, Eigen::Vector2f &linePoint, Eigen::Vector2f &lineDirection);
	}
}

#endif